/*
 * Read the buttons state
 */
#include <xqueue.h>
#include <xtask.h>
#include <rrcon.h>
#include <init_systems.h>
#include "log.h"

#include "buttons_reader.h"

# define PERIOD 10

namespace Group5 {
    using namespace kthrobot;


    ButtonsReader::ButtonsReader()
        : XTask("ButtonsReader"){
            for(int i=0; i<5; i++) {
                previous_state[i] = false;
            }
            SetPeriodHz(PERIOD);
        }


    ButtonsReader::~ButtonsReader() {
        Join();
    }


    void ButtonsReader::TaskCode() {
        while(!MustStop()) {
            uint32_t b = Buttons::Read();
            if (Buttons::Left(b) && !previous_state[LEFT_BUTTON]) {
                Log::Write("Left Button pressed");
                buttons_queue_.Enq(LEFT_BUTTON);
            }
            if (Buttons::Right(b) && !previous_state[RIGHT_BUTTON]) {
                Log::Write("Right Button pressed");
                buttons_queue_.Enq(RIGHT_BUTTON);
            }
            if (Buttons::Up(b) && !previous_state[UP_BUTTON]) {
                Log::Write("Up Button pressed");
                buttons_queue_.Enq(UP_BUTTON);
            }
            if (Buttons::Down(b) && !previous_state[DOWN_BUTTON]) {
                Log::Write("Down Button pressed");
                buttons_queue_.Enq(DOWN_BUTTON);
            }
            if (Buttons::Center(b) && !previous_state[CENTER_BUTTON]) {
                Log::Write("Button pressed");
                buttons_queue_.Enq(CENTER_BUTTON);
            }
            // Updating the state
            previous_state[LEFT_BUTTON] = Buttons::Left(b);
            previous_state[RIGHT_BUTTON] = Buttons::Right(b);
            previous_state[UP_BUTTON] = Buttons::Up(b);
            previous_state[DOWN_BUTTON] = Buttons::Down(b);
            previous_state[CENTER_BUTTON] = Buttons::Center(b);
            WaitPeriod();
        }
    }

    bool* ButtonsReader::WaitForButton() {
        // Pauses the program until the button is pressed,
        // checks every 'waitms' ms.
        bool* pointer =  new bool[5];
        bool button_pressed[5] = {0,0,0,0,0};
        int button_id = 0;
        bool done = false;
        while (!done) {
            TimeTools::WaitMS(500);

            // At each period we check every button that has been pressed
            while ( buttons_queue_.Deq(&button_id) ) {
               button_pressed[button_id] = 1;
               done = true;
            }
        }
        for(int i = 0;i<5;i++){
          pointer[i] = button_pressed[i];
        }
        return pointer;
    }
};
